package math.base;

/**
 * @author Saga
 *
 */
public class AxisAngle {
    public AxisAngle() {
    }

    public AxisAngle(final Dir3d _axis, final double _angle) {
        this.axis = _axis;
        this.angle = _angle;
    }

    public final void setAxis(final Dir3d _dir) {
        this.axis = _dir;
    }

    public final Dir3d getAxis() {
        return this.axis;
    }

    public final void setAngle(final double _angle) {
        this.angle = _angle;
    }

    public final double getAngle() {
        return this.angle;
    }

    public final Vector3d transformVector(final Vector3d _vec) {
        final double cosVal = Math.cos(angle);
        final double sinVal = Math.sin(angle);

        final Vector3d crossVec = new Vector3d(this.axis.getCoord().crossed(_vec.getCoord()));
        final Vector3d dotVec = this.axis.multiplied(this.axis.getCoord().dot(_vec.getCoord()));

        return _vec.multiplied(cosVal).added(crossVec.multiplied(sinVal)).added(
                dotVec.multiplied(1.0 - cosVal));
    }

    public static final AxisAngle createFromMatrix(final Matrix3d _mat) {
        final double trace = _mat.calcTrace();
        final double angle = Math.acos(trace * 0.5 - 0.5);

        final double sinVal = Math.sin(angle);
        return null;
    }

    public final Matrix3d toMatrix() {
        final double cosVal = Math.cos(angle);
        final double compCosVal = 1.0 - cosVal;
        final double sinVal = Math.sin(angle);

        final double xy = axis.getX() * axis.getY();
        final double yz = axis.getY() * axis.getZ();
        final double zx = axis.getZ() * axis.getX();

        final double xx = axis.getX() * axis.getX();
        final double yy = axis.getY() * axis.getY();
        final double zz = axis.getZ() * axis.getZ();

        return new Matrix3d(cosVal + compCosVal * xx, -sinVal * axis.getZ() + compCosVal * xy,
                sinVal * axis.getY() + compCosVal * zx, sinVal * axis.getZ() + compCosVal * xy,
                cosVal + compCosVal * yy, -sinVal * axis.getZ() + compCosVal * yz,
                -sinVal * axis.getY() + compCosVal * zx, sinVal * axis.getX() + compCosVal * yz,
                cosVal + compCosVal * zz);
    }

    public final Quaternion toQuaternion() {
        final double halfAngle = this.angle * 0.5;
        final double cosVal = Math.cos(halfAngle);
        final double sinVal = Math.sin(halfAngle);

        return new Quaternion(cosVal, this.axis.getX() * sinVal, this.axis.getY() * sinVal,
                this.axis.getZ() * sinVal);
    }

    @Override
    public final AxisAngle clone() {
        return new AxisAngle(this.axis.clone(), this.angle);
    }

    private Dir3d axis;
    private double angle;
}
